package behaviours;

import misc.Log;
import core.Actuators;
import core.Senses;

public class StepForward extends Behaviour {

	private static final int TOLERANCE = 900;
	private int steps = 0;
	
	@Override
	protected boolean isFinished() {
		Senses.readProximity();
		Log.debug(Senses.getProximity(2)+" "+Senses.getProximity(3));
		if( (Senses.getProximity(2) > TOLERANCE) || (Senses.getProximity(3) > TOLERANCE) )
			return true;
		return steps == 0;
	}

	@Override
	protected void loop() throws InterruptedException {
		Actuators.setMotors(10, 10);
		Actuators.setMotors(0, 0);
		steps--;
	}

	@Override
	protected void end() throws InterruptedException {
		Actuators.setMotors(0, 0);
	}

	@Override
	protected boolean handleArguments(Object[] arguments) {
		steps = 6*(Integer)arguments[0];
		return true;
	}

}
